// cartesian_position_controller.cpp
#include <franka_example_controllers/cartesian_position_controller.h>
#include <pluginlib/class_list_macros.h>
#include <cmath>
#include <stdexcept>

using namespace Eigen;

namespace franka_example_controllers {

void CartesianPositionController::targetCallback(const franka_example_controllers::LhyCartesianCmd& msg) {
  //printf("====================================================================================\n\n\n\n\n\n\n\n");
  target_position_ = Vector3d(msg.position_c[0], msg.position_c[1], msg.position_c[2]);
    has_new_target_ = true;
  cmd_id = msg.cmd_id;
  params_.max_speed = msg.max_speed_c;
  if(params_.max_speed > global_max_speed_)
  {
	  params_.max_speed = global_max_speed_;
  }

  ROS_INFO_STREAM("New target received: " << target_position_.transpose());
}

bool CartesianPositionController::init(
	hardware_interface::RobotHW* robot_hw,
	ros::NodeHandle& node_handle) {
	
	this->nh = node_handle;
	// 初始化硬件接口
	std::string arm_id;
	if (!node_handle.getParam("arm_id", arm_id)) {
		ROS_ERROR("CartesianPositionController: Missing arm_id parameter");
		return false;
	}	
	
	auto* velocity_interface = robot_hw->get<franka_hw::FrankaVelocityCartesianInterface>();
	if (!velocity_interface) {
		ROS_ERROR("CartesianPositionController: Could not get velocity interface");
		return false;
	}

	try {
	velocity_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>(
	    velocity_interface->getHandle(arm_id + "_robot"));
	} catch (const hardware_interface::HardwareInterfaceException& e) {
		ROS_ERROR_STREAM("CartesianPositionController: " << e.what());
		return false;
	}
	
	auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
	if (!state_interface) {
		ROS_ERROR("CartesianPositionController: Could not get state interface");
		return false;
	}
	
	try {
	state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(state_interface->getHandle(arm_id + "_robot"));
	} catch (const hardware_interface::HardwareInterfaceException& e) {
		ROS_ERROR_STREAM("CartesianPositionController: " << e.what());
		return false;
	}
	
	// 加载参数
	node_handle.getParam("params/kp", params_.kp);
	node_handle.getParam("params/global_max_speed", global_max_speed_);
	node_handle.getParam("params/max_acceleration", params_.max_acceleration);
	node_handle.getParam("params/deadzone", params_.deadzone);
	
	// 初始化订阅者
	target_sub_ = nh.subscribe(
	  "/cartesian_position_controller/target", 10, 
	  &CartesianPositionController::targetCallback, this);

	// 初始化状态发布者
	status_pub_ = nh.advertise<franka_example_controllers::LhyCmdState>(
		"is_running", 
		1);
	status_timer_ = nh.createTimer(
		ros::Duration(0.1),
		&CartesianPositionController::publishStatusTimerCallback,
		this);
	

	ROS_INFO("Create Catesian Position Controller");
	return true;
}
void CartesianPositionController::publishStatusTimerCallback(const ros::TimerEvent&) {
	franka_example_controllers::LhyCmdState status_msg;
	status_msg.is_running = has_new_target_;
	status_msg.cmd_id = cmd_id;
	status_pub_.publish(status_msg);
}



void CartesianPositionController::starting(const ros::Time&) {
  // 初始位置设为当前末端位置
  auto initial_pose = state_handle_->getRobotState().O_T_EE;
  current_position_ = Map<const Matrix4d>(initial_pose.data()).block<3,1>(0,3);
  target_position_ = current_position_;
  current_velocity_.setZero();
}

double CartesianPositionController::get_target_speed(double& error_norm)
{
	double no_constrain_speed = params_.kp * std::sqrt(params_.max_acceleration * error_norm);
	return std::min(no_constrain_speed, params_.max_speed);
}

void CartesianPositionController::update(const ros::Time&, 
                                        const ros::Duration& period) {
	// 获取当前末端位置
	auto current_pose = state_handle_->getRobotState().O_T_EE;
	current_position_ = Map<const Matrix4d>(current_pose.data()).block<3,1>(0,3);
	
	// 计算位置误差和残余速度 
	Eigen::Vector3d position_error = target_position_ - current_position_;
	double error_norm = position_error.norm();
	double velocity_norm = current_velocity_.norm();
	
	/************************提前退出机制*************************/ 
	// 检查是否到达目标
	if (error_norm < params_.deadzone && velocity_norm < params_.deadzone) {
		if (has_new_target_) {
			ROS_INFO("Reached target position");
			has_new_target_ = false;
		}
		std::array<double,6> command = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
		velocity_handle_->setCommand(command);
		return;
	}
	//检查是否有时间变化 
	double dt = period.toSec();
	if (dt == 0.0)
	{
		setVelocityCmd(current_velocity_);
		return;
	}
	
	/************************计算速度分解*************************/ 
	
	// 分解当前速度到目标方向和垂直方向
	Eigen::Vector3d target_direction = position_error.normalized(); 	// 计算期望速度方向
	Eigen::Vector3d desired_v = get_target_speed(error_norm) * target_direction;
	Eigen::Vector3d velocity_error = desired_v - current_velocity_;
	// double balance_coeff = std::abs(velocity_error.norm() / (position_error.norm() + 1e-3));

    // 计算最大允许速度变化量
	const double max_delta_v = params_.max_acceleration * period.toSec();

	// 合成最终速度
	Eigen::Vector3d new_velocity = current_velocity_ + 
		velocity_error.normalized() * std::min(velocity_error.norm(), max_delta_v);	
	
	// 确保不超过最大速度限制
	if (new_velocity.norm() > params_.max_speed) {
	new_velocity = new_velocity.normalized() * params_.max_speed;
	}
	
	// 设置笛卡尔速度指令（仅线速度，角速度保持为零）
	setVelocityCmd(new_velocity);
	
	current_velocity_ = new_velocity;
}


void CartesianPositionController::stopping(const ros::Time&) {
	//ROS_INFO("Released Catesian Position Controller");
}

void CartesianPositionController::setVelocityCmd(const Eigen::Vector3d& velocity_cmd){
	std::array<double, 6> command = {
		velocity_cmd.x(),
		velocity_cmd.y(),
		velocity_cmd.z(),
		0.0, 0.0, 0.0
	};
	velocity_handle_->setCommand(command);	
}

PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianPositionController,
                       controller_interface::ControllerBase)
}

